#include "ImageDataBase.h"

#define SCORE_THRESHOLD 2.0
#define MIN_MATCHES 80

void CImageDataBase::LoadImageData(char* ImageDataFile, ImageNamesVector ImagesName)
{
	FILE* pFile = fopen(ImageDataFile, "r");
	VISIO_ASSERT(pFile);


	fscanf(pFile, "%d", &m_NumImages);
	VISIO_ASSERT(m_NumImages > 0)

	for (UINT i = 0; i < m_NumImages; i++)
	{
		KeyPointVector newKeyPoints;
		UINT NumKeys = 0;
		double InitFocal = 0;

		fscanf(pFile, "%d %lf", &NumKeys, &InitFocal);
		
		for (UINT j = 0; j < NumKeys; j++)
		{
			double x, y;
			int r, g, b;
			fscanf(pFile, "%lf %lf %d %d %d", &x, &y, &r, &g, &b);
			KeyPoint2D newKeyPoint(x, y, r, g, b);
			newKeyPoints.push_back(newKeyPoint);
		}

		CImageData newImageData(newKeyPoints, ImagesName[i].c_str(), i, InitFocal);
		m_ImageData.push_back(newImageData);
	}
}

//for each image this will set up her track
void CImageDataBase::SetTracks( CTrackDataBase* pTracks )
{
	m_pTrackDataBase = pTracks;
	for (UINT TrackIndex = 0; TrackIndex < m_pTrackDataBase->GetNumOfTracks(); TrackIndex++)
	{
		CTrackData* pTrack = m_pTrackDataBase->GetTrack(TrackIndex);
		for (UINT ViewIndex = 0; ViewIndex < pTrack->GetNumViews(); ViewIndex++)
		{
			View view = pTrack->GetView(ViewIndex);

			//Keypoint KeyIndex from ImageIndex is a part of the Track
			UINT ImageIndex = view.first;
			UINT KeyIndex = view.second;
			
			m_ImageData[ImageIndex].SetVisablePoint(pTrack, KeyIndex);
		}
	}
}
CImageMatch* CImageDataBase::GetMatchFromIndex(UINT i, UINT j) const
{
	MatchesMap::const_iterator it = m_MatchesMap.find(ImagePairIndex(i,j));

	if (it != m_MatchesMap.end())
	{
		return it->second;
	}
	else
	{
		return NULL;
	}
}

CImageMatch* CImageDataBase::GetMatchFromIndex( ImageIndexMatch match ) const
{
	return GetMatchFromIndex(match.first, match.second);
}

ImageIndexMatch CImageDataBase::GetBestInitialPair()
{
	ImageIndexMatch BestMatch(-1,-1);
	ImageIndexMatch BestMatchBackUp(-1,-1);
	double MaxScore = 0;
	UINT MaxMatches = 0;

	for (MatchesMap::iterator it = m_MatchesMap.begin(); it != m_MatchesMap.end(); it++)
	{
		CImageMatch* pMatch = it->second;
		if (pMatch && pMatch->GetNumMatches() > MaxMatches && pMatch->GetInlinersScore() > SCORE_THRESHOLD)
		{
			MaxMatches = pMatch->GetNumMatches();
			BestMatch = pMatch->GetImageIndices();
		}

		if (pMatch && pMatch->GetNumMatches() > MIN_MATCHES && pMatch->GetInlinersScore() > MaxScore) 
		{
			MaxScore = pMatch->GetInlinersScore();
			BestMatchBackUp = pMatch->GetImageIndices();
		}
	}


	if (BestMatch == ImageIndexMatch(-1,-1))
	{
		if (BestMatchBackUp == ImageIndexMatch(-1, -1))
		{
			VISIO_ASSERT(0);
		}
		else
		{
			return BestMatchBackUp;
		}
	}
	else
	{
		return BestMatch;
	}
}

UINT CImageDataBase::GetNumImages() const
{
	VISIO_ASSERT(m_NumImages == m_ImageData.size());
	return m_NumImages;
}

void CImageDataBase::LoadMatches( char* MatchesFile )
{
	VISIO_ASSERT(!m_ImageData.empty());
	VISIO_ASSERT(m_pTrackDataBase);
	int NumMatches = 0;


	FILE* pFile = fopen(MatchesFile, "r");
	fscanf(pFile, "%d", &NumMatches);
	for (int i = 0; i < NumMatches; i++)
	{
		int idx1, idx2;
		int NumInliners = 0;
		double InlinersRatio = 0;
		// format of file is first the 2 indices of the images then the (inliners ratio(???)) then the number of inliners  
		fscanf(pFile, "%d %d", &idx1, &idx2);
		fscanf(pFile, "%lf", &InlinersRatio);
		fscanf(pFile, "%d", &NumInliners);
	

		CImageMatch* pImageMatch = new CImageMatch(GetImageData(idx1), GetImageData(idx2), NumInliners, InlinersRatio);
		
		MatchMapInsertReturnType ret = m_MatchesMap.insert(MatchMapElement(pImageMatch->GetImageIndices(), pImageMatch));
		if (!ret.second && (ret.first->second->GetNumMatches() == 0) && (pImageMatch->GetNumInliners() != 0))
		{
			//the already existent match is the real one has bad values replace it
			m_MatchesMap[pImageMatch->GetImageIndices()] = pImageMatch;
		}
	}

}

CImageData* CImageDataBase::GetImageData( UINT index )
{
	VISIO_ASSERT(index < GetNumImages());
	return &m_ImageData[index];
}

UINT CImageDataBase::GetNumTracksCorespanding( UINT index, VisiblePointsVector& VisablePoints )
{
	CImageData* pImageDate = GetImageData(index);
	for (VisablePointItr it = pImageDate->VisablePointsBegin(); it != pImageDate->VisablePointsEnd(); it++)
	{
		if (it->first->IsMarked())
		{
			VisablePoints.push_back(*it);
		}
	}

	return VisablePoints.size();
}

SharedViewVector CImageDataBase::GetTracksShared( ImageDataPtrVector ImageDataptr )
{
	CameraIndexVector CamIndices;
	SharedViewVector retViews; 
	for (UINT i = 0; i < ImageDataptr.size(); i++)
	{
		CamIndices.push_back(ImageDataptr[i]->GetImageIndex());
	}

	for (UINT i = 0; i < m_pTrackDataBase->GetNumOfTracks(); i++)
	{
		CTrackData* pTrack = m_pTrackDataBase->GetTrack(i);

		if (pTrack->IsMarked() || pTrack->IsDirty())
		{
			// already seen this track
			continue;
		}

		SSharedView SharedView = pTrack->IsVisable(CamIndices);
		if (!SharedView.views.empty())
		{
			retViews.push_back(SharedView);
		}
	}

	return retViews;
}
